/*
 * KatrinaPanel.h
 *
 *  Created on: 6 May 2017
 *      Author: yiming
 */

#ifndef KATRINA_GUI_INCLUDE_KATRINA_GUI_KATRINAPANEL_H_
#define KATRINA_GUI_INCLUDE_KATRINA_GUI_KATRINAPANEL_H_

#include <ros/ros.h>
#include <ros/package.h>
#include <rviz/panel.h>
#include <tf/transform_listener.h>
#include <katrina_gui/LocationManager.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <actionlib_msgs/GoalID.h>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QPushButton>
#include <QComboBox>
#include <QLabel>
#include <QSpacerItem>
#include <QInputDialog>
#include <QMessageBox>
#include <QLine>
#include <QListWidget>
#include <QFileDialog>
#include <QProgressBar>
#include <QRadioButton>

#include <nav_msgs/Odometry.h>
#include <std_srvs/Empty.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdlib.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/UInt32MultiArray.h>

using namespace Katrina::Navigation;

namespace Katrina
{
    namespace Navigation
    {
        enum NavStatus
        {
            IDLE = 0, SUCCESS = 10, INPROGRESS = 20, FAILED = 30, SELF_LOCALIZATION = 40, WAIT_APPLICATION = 50
        };
    }

    namespace Visualization
    {
        class KatrinaPanel : public rviz::Panel
        {
            Q_OBJECT
            public:
                KatrinaPanel(QWidget* parent = 0);
                virtual ~KatrinaPanel();
            public Q_SLOTS:
                void localizeCallback();
                void addLocationCallback();
                void addMouseLocationCallback();
                void removeLocationCallback();
                void teleopCallback();
                void goToLocationCallback();
                void stopCallback();
                void odomCallback(const nav_msgs::OdometryConstPtr &msg);
                void removeCruiseCallback();
                void openCruiseFile();
                bool saveCruiseFile();
                void startCruising();
                void setManualCruiseSetup(bool enable);
                void clearCruise();

                void autoChargeButtonCallback();
                void manualautoChargeButtonCallback();
                void charge();
            private:
                void getCurrentLocation(geometry_msgs::Pose &pose);
                bool addLocation(const geometry_msgs::Pose &pose, const std::string &name);
                void publishLocationsToRviz();
                void addLine(QBoxLayout* layout);
                void setRobotStatus(const NavStatus &status);
                bool loadCruisePath(const std::string &file);
                void moveToLocation(const KnownLocation &location);
		void cancelGoal();
                inline bool checkFileExist(const std::string& name)
                {
                    struct stat buffer;
                    return (stat(name.c_str(), &buffer) == 0);
                }
                void moveBaseResultCallback(const move_base_msgs::MoveBaseActionResultConstPtr &msg);
                void finishApplicationCallback(const std_msgs::BoolConstPtr &msg);
                void goToNextCruiseLocation();
                void mouseCruiseLocationCallback(const geometry_msgs::PoseStampedConstPtr &msg);
                void addToCruiseCallback(const KnownLocation &location);
                void selfLocalizationCallback(const std_msgs::BoolConstPtr &msg);
                void preCharge();
                ros::NodeHandle nh_;
                ros::Publisher location_mark_pub_;
                ros::Publisher nav_stop_pub_;
                ros::Subscriber odom_sub_;
                ros::Publisher application_start_pub_;
                ros::Subscriber application_finish_sub_;
                nav_msgs::Odometry odom_;
                visualization_msgs::MarkerArray location_marks_;
                LocationManagerPtr location_manager_;
                QComboBox* location_box_;
                QListWidget* location_widget_;
                QLineEdit* status_line_;
                std::string location_file_path_;
                QPushButton* cruise_remove_button_;
                QPushButton* cruise_open_button_;
                QPushButton* cruise_save_button_;

                actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client_;
                ros::Publisher simple_goal_pub_;
                ros::Publisher cruise_number_pub_;
                ros::Publisher cruise_path_pub_;
                visualization_msgs::MarkerArray cruise_number_mark_;
                visualization_msgs::Marker cruise_path_mark_;
                std::vector<KnownLocation> cruise_path_;
                ros::Subscriber move_base_status_sub_;
                bool in_cruise_mode_;
                int current_cruise_index_;
                ros::Subscriber mouse_cruise_location_sub_;
                ros::Subscriber self_localization_sub_;

                void batteryCallback(const std_msgs::Float32MultiArrayConstPtr &msg);
                void chargeFinishCallback(const std_msgs::BoolConstPtr &msg);
                ros::Subscriber battery_sub_;
                ros::Publisher start_charge_pub_;
                ros::Subscriber finish_charge_sub_;
                QProgressBar *battery_bar_;
                QRadioButton *auto_charge_button_;
                QPushButton *manual_charge_button_;
                KnownLocation charge_station_;
                bool in_charge_mode_;
		bool autoChargeEnable;
		bool in_autoCharge_ = true;
		bool charge_flag = false;
        };
    }
}

#endif /* KATRINA_GUI_INCLUDE_KATRINA_GUI_KATRINAPANEL_H_ */
